#!/usr/bin/env python3
"""
使用重构后的ArucoTracker的示例代码

本示例展示了如何使用基于线程的ArucoTracker进行标记跟踪，
而不是使用回调函数的方式。
"""

import sys
import os
import cv2
import numpy as np
import time
from threading import Event

# 添加模块路径
current_dir = os.path.dirname(os.path.abspath(__file__))
parent_dir = os.path.dirname(current_dir)
sys.path.insert(0, parent_dir)

from aruco_tracker import ArucoTracker, ArucoMarker


def user_callback(tracker):
    """
    用户回调函数，在检测到标记时被调用
    
    Args:
        tracker: ArucoTracker实例
    """
    # 获取跟踪统计信息
    stats = tracker.get_tracking_statistics()
    
    # 获取所有标记状态
    all_markers = tracker.get_all_markers_status()
    
    # 打印统计信息
    print(f"\r跟踪统计: FPS={stats['fps']:.1f}, "
          f"已跟踪={stats['tracked_markers']}/{stats['total_markers']}, "
          f"检测次数={stats['detection_count']}", end='')
    
    # 打印跟踪到的标记信息
    for name, status in all_markers.items():
        if status['is_tracked']:
            pos = status['position']
            rot = status['rotation']
            print(f"\n{name}: pos=({pos[0]:.1f}, {pos[1]:.1f}, {pos[2]:.1f}), "
                  f"rot=({rot[0]:.1f}, {rot[1]:.1f}, {rot[2]:.1f})", end='')


def main():
    """
    主函数，演示ArucoTracker的使用
    """
    print("=== ArucoTracker 线程模式示例 ===")
    
    # 创建ArucoTracker实例
    calibration_file = "../../calibration/camera_params_20250715_203926.json"
    tracker = ArucoTracker(
        camera_index=0,
        calibration_file=calibration_file,
        max_lost_frames=30
    )
    
    # 创建ArUco标记（使用默认的滑动窗口大小5来减少检测噪声）
    marker1 = ArucoMarker("target_1", 0, 50.0)  # ID=0, 大小=50mm
    
    # 注册标记
    tracker.register_marker(marker1)
    # tracker.register_marker(marker2)
    # tracker.register_marker(marker3)
    
    # 设置用户回调函数
    tracker.set_callback(user_callback)
    
    try:
        # 开始跟踪
        print("开始跟踪...")
        if not tracker.start_tracking():
            print("跟踪启动失败")
            return
        
        # 等待跟踪开始
        if not tracker.wait_for_tracking_start():
            print("跟踪启动超时")
            return
        
        print("跟踪已开始，按 Ctrl+C 停止...")
        
        # 创建停止事件
        stop_event = Event()
        
        # 主循环
        frame_count = 0
        start_time = time.time()
        
        while not stop_event.is_set():
            try:
                # 检查跟踪状态
                if not tracker.is_tracking_active():
                    print("\n跟踪已停止")
                    break
                
                # # 每隔一段时间显示相对位置信息
                # if frame_count % 30 == 0:  # 每30帧显示一次
                #     relative_info = tracker.get_relative_position("target_1", "reference")
                #     if relative_info:
                #         rel_pos = relative_info['relative_position']
                #         rel_dist = relative_info['relative_distance']
                #         print(f"\ntarget_1相对于reference的位置: "
                #               f"({rel_pos[0]:.1f}, {rel_pos[1]:.1f}, {rel_pos[2]:.1f}), "
                #               f"距离: {rel_dist:.1f}mm")
                
                # 可选：获取当前帧进行显示
                current_frame = tracker.get_current_frame()
                if current_frame is not None:
                    # 这里可以进行图像显示或其他处理
                    cv2.imshow('Current Frame', current_frame)
                    cv2.waitKey(1)
                    pass
                
                frame_count += 1
                time.sleep(0.01)  # 短暂休眠，避免CPU占用过高
                
            except KeyboardInterrupt:
                print("\n收到停止信号...")
                break
        
        # 显示最终统计信息
        runtime = time.time() - start_time
        stats = tracker.get_tracking_statistics()
        print(f"\n运行时间: {runtime:.1f}秒")
        print(f"平均FPS: {stats['fps']:.1f}")
        print(f"总检测次数: {stats['detection_count']}")
        
        # 保存跟踪数据
        # tracker.save_tracking_data("tracking_data.json")
        
    except Exception as e:
        print(f"运行错误: {e}")
        
    finally:
        # 停止跟踪
        print("停止跟踪...")
        tracker.stop_tracking()
        print("程序结束")


if __name__ == "__main__":
    main()
